// Robot.cpp
//
// Robot Class

#include "Robot.h"

// DEFAULT CONSTRUCTOR
Robot::Robot( char * fileName, int id, Pathfinder & pather, Grid<char> & occGrid,
			 float walkSpeed, float turnSpeed)
			 : Agent( fileName, id, pather, occGrid, walkSpeed, turnSpeed ), isFlipped( false )
{
	speed = 0;
	currPath = list<Triple>();
	traverser = PathTraverser( & currPath, walkSpeed, turnSpeed );
}

// DEFAULT DESTRUCTOR
Robot::~Robot()
{
}

/*
Want the 'goal' xyz coordinate to be defined by user.
*/
void Robot::moveTo(Triple goal)
{
	// mark current location as clear.
	Triple origPosition = getPosition();
	Position gridOrigPosition = pathfinder.posToGrid(origPosition);
	occGrid(gridOrigPosition.x, gridOrigPosition.z) = FALSE;

	currPath.clear();
	currPath = pathfinder.navigate( getPosition(), goal );
	traverser.setPath( & currPath, getRotation() );
	setState( Walking );
	changedState = true;

	// get current position
	Triple p = getPosition();
	Position op = pathfinder.posToGrid(p);

	// color occupancy grid
	int tmpID = 5700;
	dbMakeObjectPlane		( tmpID , 30, 30 );
	dbXRotateObject			( tmpID , -90 );
	dbPositionObject		( tmpID , op.x, 0.5, op.z);
	dbColorObject			( tmpID, dbRGB(255,0,0) );
	dbSetObjectCollisionOff ( tmpID );
	dbGhostObjectOn( tmpID );

	// mark occupancy grid as occupied by robot.
	occGrid(op.x, op.z) = 4;
}

// if see X, do Y.
// SEE: void Agent::update()

/*	int objID = dbPickObject(dbScreenWidth()/2, dbScreenHeight()/2, 0, 9000); // mouse centered
if (objID != 0)
{
dbSetObjectAmbience(objID, dbRGB(255,0,0));
agent = agents.getAgent(objID);
}
*/

/*
Checks to see if an enemy is near.
*/
bool Robot::enemyNear()
{
	bool killzone = false;				// default value set to false.
	int distance = 30;					// distance our dude can "see"

	Position squaresInLOS[5];			// array to store 3D values in line-of-sight
	Triple gridSquaresInLOS[5];			// DEBUG: array to store grid values in line-of-sight

	// get current robot position
	Triple p = getPosition();

	// calculate various offsets for N/W/E/NW/NE
	float direction = dbWrapValue(getRotY());
	float newX = dbNewXValue( p.x, direction, distance );
	float newZ = dbNewZValue( p.z, direction, distance );
	Triple N = Triple( newX, p.y, newZ );

	direction = dbWrapValue(getRotY() - 90 );
	newX = dbNewXValue( p.x, direction, distance );
	newZ = dbNewZValue( p.z, direction, distance );
	Triple W = Triple( newX, p.y, newZ );

	direction = dbWrapValue(getRotY() + 90);
	newX = dbNewXValue( p.x, direction, distance );
	newZ = dbNewZValue( p.z, direction, distance );
	Triple E = Triple( newX, p.y, newZ );

	direction = dbWrapValue(getRotY() - 45);
	newX = dbNewXValue( p.x, direction, distance );
	newZ = dbNewZValue( p.z, direction, distance );
	Triple NW = Triple( newX, p.y, newZ );

	direction = dbWrapValue(getRotY() + 45);
	newX = dbNewXValue( p.x, direction, distance );
	newZ = dbNewZValue( p.z, direction, distance );
	Triple NE = Triple( newX, p.y, newZ );

	// build 3D line-of-sight vector with offset of distance
	squaresInLOS[0] = pathfinder.posToGrid(N);				// check N
	squaresInLOS[1] = pathfinder.posToGrid(W);				// check W
	squaresInLOS[2] = pathfinder.posToGrid(E);				// check E
	squaresInLOS[3] = pathfinder.posToGrid(NW);				// check NW
	squaresInLOS[4] = pathfinder.posToGrid(NE);				// check NE

	// DEBUG: build GRID line-of-sight vector with offset of distance
	gridSquaresInLOS[0] = N;
	gridSquaresInLOS[1] = W;
	gridSquaresInLOS[2] = E;
	gridSquaresInLOS[3] = NW;
	gridSquaresInLOS[4] = NE;

	// DEBUG: color grids we are checking
	for (int i=0; i<5; i++)
	{
		int tmpID = 5500+i;
		dbMakeObjectPlane		( tmpID , 30, 30 );
		dbXRotateObject			( tmpID , -90 );
		dbPositionObject		( tmpID , gridSquaresInLOS[i].x, 0.5, gridSquaresInLOS[i].z);
		dbColorObject			( tmpID, dbRGB(255,0,0) );
		dbSetObjectCollisionOff ( tmpID );
		dbGhostObjectOn( tmpID );
	}

	// check line-of-sight vector.
	for (int i=0; i<5; i++)
	{
		// TODO: check for occlusion
		int foo = occGrid(squaresInLOS[i].x, squaresInLOS[i].z);

		if (foo == 9)
		{
			setState(Attacking);
			killzone = true;
		}
	}

	return killzone;
}

void Robot::update()
{
	oldPose = newPose;

	if (!currPath.empty())
	{
		newPose = traverser.update();
		Object::setPosition( newPose.position );
		PoseableObject::setRotation( newPose.rotation );
	}

	oldState = getState();
	State newState;

	if( currPath.size() == 1 || oldPose == newPose )
	{
		setState( Idle );
		changedState = oldState == getState() ? false : true;
	}
	else
	{
		setState( Walking );
	}

	if (enemyNear() == true)
	{
		dbSetObjectAmbience(this->getID(), dbRGB(255,0,0));
		// change state, engage?
		setState(Attacking);
		changedState = oldState == getState() ? false : true;
	}

	newState = getState();
	if (changedState) {
		switch( newState )
		{
		case Robot::Walking:
			playAnimation(Animation::Move );
			break;
		case Robot::Idle:
			playAnimation(Animation::Idle );
			break;
		case Robot::Attacking:
			playAnimation(Animation::Attack );
			// trigger event for hostile to die.
			break;
		default:
			break;
		}

		changedState = false;
	}
}
// TODO occupancy grid.
// before move character, set current square to char zero.
// move them, and then set new square to number corresponding to entity.

// get robot current position.
// Position pos = pathfinder.posToGrid( getPosition() );

// mark current position with free space identifier. (0).
// Agent::occGrid(pos.x, pos.y) = 0;

// do move

// post move, mark new grid location with ROBOT identifier, 4.
// Agent::occGrid(pos.x, pos.y) = 4;



// pathfinder.occupancyGrid
// DRAW ROBOT ON MINIMAP HERE!!

/*
int currState = 0;
// if someone is within 1 occupancy grid, change state to die.
currState = getState();

bool changedState = false;

setState(Idle);

if (currState != Idle) {
changedState = true;
}
*/